#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
#include "../include/cal_obs/GPS2ENU.h"
#include <std_msgs/msg/float64_multi_array.hpp>


#define M_PI    3.14159265358979323846
#define RAND_MAX 0x7fff

struct SCGPositionGEO {
    double longitude; // 经度
    double latitude;  // 纬度
    double altitude;  // 高度
};

struct SCGAttitude {
    double roll;   // 横滚角 (rad)
    double pitch;  // 俯仰角 (rad)
    double yaw;    // 偏航角 (rad)
};

struct SCGVelocityECS {
    double x; // x方向速度 (m/s)
    double y; // y方向速度 (m/s)
    double z; // z方向速度 (m/s)
};

struct ObstructionStruct {
    int shape;                // 障碍物形状 (1-球形，2-长方体，3-柱形，4-环形)
    SCGPositionGEO position; // 障碍物位置
    SCGVelocityECS velocity; // 障碍物速度
    SCGAttitude attitude;    // 障碍物姿态
    double paraA;            // 障碍物参数A
    double paraB;            // 障碍物参数B
    double paraC;            // 障碍物参数C

    std::vector<std::vector<double>> computeSurfacePoints(double dis);
};

class ObstaclePublisher : public rclcpp::Node
{
public:
    ObstaclePublisher()
    : Node("obstacle_publisher")
    {   
        this->declare_parameter<std::vector<double>>("cal_obs_param", std::vector<double>{});
        this->declare_parameter<std::vector<double>>("calculated_obstacles", std::vector<double>{});
        cal_obs_param = this->get_parameter("cal_obs_param").as_double_array();
        param_subscriber_ = std::make_shared<rclcpp::ParameterEventHandler>(this);
        // Set a callback for this node's integer parameter, "age"
        auto cb = [this](const rclcpp::Parameter & p) {
            cal_obs_param = p.as_double_array();
            obs_num = static_cast<int>(cal_obs_param[0]);
                // 更新障碍物点数据
                update_obstacles();
          };
        cb_handle_ = param_subscriber_->add_parameter_callback("cal_obs_param", cb);
        
        obs_num = int(cal_obs_param[0]);

        update_obstacles();
    }

private:
    void update_obstacles()
        {
            RCLCPP_INFO(this->get_logger(), "Clear here!\n");
            obs_points.clear();  // 清空之前计算的结果
            RCLCPP_INFO(this->get_logger(), "Start update obstacle points!\n");
            for (size_t i = 1; i < obs_num*7; i += 7) {
                int shape = static_cast<int>(cal_obs_param[i]);
                double pos_x = cal_obs_param[i + 1];
                double pos_y = cal_obs_param[i + 2];
                double pos_z = cal_obs_param[i + 3];
                double paraA = cal_obs_param[i + 4];
                double paraB = cal_obs_param[i + 5];
                double paraC = cal_obs_param[i + 6];
                RCLCPP_INFO(this->get_logger(), "Element %f, %f, %f, %f, %f, %f, %f\n", pos_x, pos_y, pos_z, paraA, paraB, paraC);
                ObstructionStruct obstruction;
                SCGPositionGEO drone_p;
                obstruction.shape = shape;
                obstruction.position = { pos_x, pos_y, pos_z };
                obstruction.attitude = { 0,0,0 }; // 示例姿态 (以弧度表示)
                obstruction.paraA = paraA; // 球形半径
                obstruction.paraB = paraB; // 不适用
                obstruction.paraC = paraC; // 不适用
                auto surfacePoints = obstruction.computeSurfacePoints(0.1);
                for(int j=0;j<surfacePoints.size();j++){
                obs_points.push_back(surfacePoints[j][0]);
                obs_points.push_back(surfacePoints[j][1]);
                obs_points.push_back(surfacePoints[j][2]);
                }
            }

            // 更新ROS中的参数
            this->set_parameter(rclcpp::Parameter("calculated_obstacles", obs_points));
        }
    

    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_;
    std::shared_ptr<rclcpp::ParameterEventHandler> param_subscriber_;
    std::shared_ptr<rclcpp::ParameterCallbackHandle> cb_handle_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::vector<double> cal_obs_param;
    int obs_num;
    std::vector<double> obs_points;
};

// 计算两个向量的点积
double dotProduct(const std::vector<double>& v1, const std::vector<double>& v2) {
    double result = 0.0;
    for (size_t i = 0; i < v1.size(); ++i) {
        result += v1[i] * v2[i];
    }
    return result;
}


std::vector<std::vector<double>> GetFrontPoints(std::vector<std::vector<double>> points, SCGPositionGEO drone_p, SCGPositionGEO obs_p) {
    std::vector<std::vector<double>> new_point;
    std::vector<double> ref_vec = { drone_p.latitude - obs_p.latitude ,drone_p.longitude - obs_p.longitude };
    for (auto p : points) {
        std::vector<double> vec = { p[1] - obs_p.latitude, p[0] - obs_p.longitude };
        double dot = dotProduct(ref_vec, vec);

        if (dot >= 0) {  // 判断夹角是否在 [-π/2, π/2] 范围内
            new_point.push_back(p);
        }
    }
    return new_point;

}

std::vector<std::vector<double>> ObstructionStruct::computeSurfacePoints(double dis) {
    std::vector<std::vector<double>> points;

    // 计算旋转矩阵
    double yawRad = attitude.yaw * M_PI / 180.0;
    double pitchRad = attitude.pitch * M_PI / 180.0;
    double rollRad = attitude.roll * M_PI / 180.0;

    double cy = cos(yawRad);
    double sy = sin(yawRad);
    double cp = cos(pitchRad);
    double sp = sin(pitchRad);
    double cr = cos(rollRad);
    double sr = sin(rollRad);
    double R[3][3] = {
    {cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr},
    {sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr},
    {-sp, cp * sr, cp * cr}
    };
    // 根据障碍物的形状生成表面点
    if (shape == 1) { // 球形
        int numPoints = static_cast<int>(4 * M_PI * paraA * paraA / (dis * dis)); // 估算点的数量
        double x, y, z;
        for (int i = 0; i < numPoints; ++i) {
            double u = static_cast<double>(i) / numPoints; // [0,1]
            double theta = acos(1 - 2 * u);               // 0到π之间均匀分布
            double phi = M_PI * (1 + sqrt(5)) * i;      // 黄金角

            x = paraA * sin(theta) * cos(phi);
            y = paraA * sin(theta) * sin(phi);
            z = paraA * cos(theta);

            // 应用旋转矩阵


            double rotatedX = R[0][0] * x + R[0][1] * y + R[0][2] * z;
            double rotatedY = R[1][0] * x + R[1][1] * y + R[1][2] * z;
            double rotatedZ = R[2][0] * x + R[2][1] * y + R[2][2] * z;
            points.push_back({ rotatedX + position.longitude, rotatedY + position.latitude, rotatedZ + position.altitude });
        }
    }
    else if (shape == 2) {
        std::vector<std::vector<double>> temp_position;
        double length = paraA;
        double width = paraB;
        double height = paraC;
        int numLength = static_cast<int>(length / dis);
        int numWidth = static_cast<int>(width / dis);
        int numHeight = static_cast<int>(height / dis);

        // 计算每个方向的步长
        double stepLength = length / numLength;
        double stepWidth = width / numWidth;
        double stepHeight = height / numHeight;

        // 在每个面生成点
        // 前面 (z = + height/2)
        for (int i = 0; i <= numLength; ++i) {
            for (int j = 0; j <= numWidth; ++j) {
                temp_position.push_back({ -length / 2 + i * stepLength,
                                  -width / 2 + j * stepWidth,
                                  height / 2 });
            }
        }

        // 后面 (z = - height/2)
        for (int i = 0; i <= numLength; ++i) {
            for (int j = 0; j <= numWidth; ++j) {
                temp_position.push_back({ -length / 2 + i * stepLength,
                                  -width / 2 + j * stepWidth,
                                  -height / 2 });
            }
        }

        // 左面 (x = - length/2)
        for (int i = 0; i <= numHeight; ++i) {
            for (int j = 0; j <= numWidth; ++j) {
                temp_position.push_back({ -length / 2,
                                  -width / 2 + j * stepWidth,
                                  -height / 2 + i * stepHeight });
            }
        }

        // 右面 (x = + length/2)
        for (int i = 0; i <= numHeight; ++i) {
            for (int j = 0; j <= numWidth; ++j) {
                temp_position.push_back({ length / 2,
                                  -width / 2 + j * stepWidth,
                                  -height / 2 + i * stepHeight });
            }
        }

        // 上面 (y = + width/2)
        for (int i = 0; i <= numLength; ++i) {
            for (int j = 0; j <= numHeight; ++j) {
                temp_position.push_back({ -length / 2 + i * stepLength,
                                  width / 2,
                                  -height / 2 + j * stepHeight });
            }
        }

        // 下面 (y = - width/2)
        for (int i = 0; i <= numLength; ++i) {
            for (int j = 0; j <= numHeight; ++j) {
                temp_position.push_back({ -length / 2 + i * stepLength,
                                  -width / 2,
                                  -height / 2 + j * stepHeight });
            }
        }

        for (int i = 0; i < temp_position.size(); i++) {
            double rotatedX = R[0][0] * temp_position[i][0] + R[0][1] * temp_position[i][1] + R[0][2] * temp_position[i][2];
            double rotatedY = R[1][0] * temp_position[i][0] + R[1][1] * temp_position[i][1] + R[1][2] * temp_position[i][2];
            double rotatedZ = R[2][0] * temp_position[i][0] + R[2][1] * temp_position[i][1] + R[2][2] * temp_position[i][2];
            points.push_back({ rotatedX + position.longitude, rotatedY + position.latitude, rotatedZ + position.altitude });
        }
    }
    else if (shape == 3) {
        double height = paraA; // 圆柱体的高度
        double radius = paraB;  // 圆柱体的半径
        double centerLongitude = position.longitude; // 圆柱体质心经度
        double centerLatitude = position.latitude;   // 圆柱体质心纬度
        double centerAltitude = position.altitude;   // 圆柱体质心高度

        // 计算圆柱体的周长
        double circumference = 2 * M_PI * radius;

        // 计算每层的点数
        int numPointsPerLayer = static_cast<int>(std::round(circumference / dis));

        // 计算每层的高度
        int numLayers = static_cast<int>(std::round(height / dis));
        double layerHeight = height / numLayers;

        for (int layer = 0; layer <= numLayers; ++layer) {
            double z = centerAltitude - (height / 2.0) + layer * layerHeight; // 当前层的高度

            // 计算圆周上的点
            for (int i = 0; i < numPointsPerLayer; ++i) {
                double angle = (2.0 * M_PI / numPointsPerLayer) * i; // 计算角度
                double longitude = centerLongitude + (radius * cos(angle)); // 计算经度
                double latitude = centerLatitude + (radius * sin(angle));   // 计算纬度

                double rotatedX = R[0][0] * longitude + R[0][1] * latitude + R[0][2] * z;
                double rotatedY = R[1][0] * longitude + R[1][1] * latitude + R[1][2] * z;
                double rotatedZ = R[2][0] * longitude + R[2][1] * latitude + R[2][2] * z;
                points.push_back({ rotatedX, rotatedY, rotatedZ });
            }
        }
        // 生成圆柱体上表面的点
        double upperZ = centerAltitude + (height / 2.0); // 上表面的高度
        for (double r = 0; r <= radius; r += dis) { // 在半径内按 dis 生成点
            int pointsInCurrentCircle = static_cast<int>(std::round(2 * M_PI * r / dis));
            for (int i = 0; i < pointsInCurrentCircle; ++i) {
                double angle = (2.0 * M_PI / pointsInCurrentCircle) * i; // 计算角度
                double longitude = centerLongitude + (r * cos(angle)); // 计算经度
                double latitude = centerLatitude + (r * sin(angle));   // 计算纬度

                double rotatedX = R[0][0] * longitude + R[0][1] * latitude + R[0][2] * upperZ;
                double rotatedY = R[1][0] * longitude + R[1][1] * latitude + R[1][2] * upperZ;
                double rotatedZ = R[2][0] * longitude + R[2][1] * latitude + R[2][2] * upperZ;

                points.push_back({ rotatedX, rotatedY, rotatedZ });
            }
        }

        // 生成圆柱体下表面的点
        double lowerZ = centerAltitude - (height / 2.0); // 下表面的高度
        for (double r = 0; r <= radius; r += dis) { // 在半径内按 dis 生成点
            int pointsInCurrentCircle = static_cast<int>(std::round(2 * M_PI * r / dis));
            for (int i = 0; i < pointsInCurrentCircle; ++i) {
                double angle = (2.0 * M_PI / pointsInCurrentCircle) * i; // 计算角度
                double longitude = centerLongitude + (r * cos(angle)); // 计算经度
                double latitude = centerLatitude + (r * sin(angle));   // 计算纬度

                double rotatedX = R[0][0] * longitude + R[0][1] * latitude + R[0][2] * lowerZ;
                double rotatedY = R[1][0] * longitude + R[1][1] * latitude + R[1][2] * lowerZ;
                double rotatedZ = R[2][0] * longitude + R[2][1] * latitude + R[2][2] * lowerZ;

                points.push_back({ rotatedX, rotatedY, rotatedZ });
            }
        }

    }
    else if (shape == 4) {
        std::vector<std::vector<double>> temp_position;
        double innerRadius = paraA;
        double outerRadius = paraB;
        double thickness = paraC;


        for (double z = 0; z <= thickness; z += dis) {
            double radius = (innerRadius + outerRadius) / 2; // 中间半径
            int numPoints = static_cast<int>((2 * M_PI * radius) / dis); // 计算每个面上的点数
            for (int i = 0; i < numPoints; ++i) {
                double angle = (2 * M_PI / numPoints) * i;
                double x = radius * cos(angle);
                double y = radius * sin(angle);
                temp_position.push_back({ x, y, z });
            }
        }

        double angleIncrement = dis / ((outerRadius + innerRadius) / 2); // 统一角度间隔
        for (double z = 0; z <= thickness; z += dis) {
            for (double radius = innerRadius; radius <= outerRadius; radius += dis) {
                int numPoints = static_cast<int>((2 * M_PI * radius) / dis); // 根据当前半径计算点数
                for (int i = 0; i < numPoints; ++i) {
                    double angle = (2 * M_PI / numPoints) * i;
                    double x = radius * cos(angle);
                    double y = radius * sin(angle);
                    temp_position.push_back({ x, y, z });
                }
            }
        }
        for (int i = 0; i < temp_position.size(); i++) {
            double rotatedX = R[0][0] * temp_position[i][0] + R[0][1] * temp_position[i][1] + R[0][2] * temp_position[i][2];
            double rotatedY = R[1][0] * temp_position[i][0] + R[1][1] * temp_position[i][1] + R[1][2] * temp_position[i][2];
            double rotatedZ = R[2][0] * temp_position[i][0] + R[2][1] * temp_position[i][1] + R[2][2] * temp_position[i][2];
            points.push_back({ rotatedX + position.longitude, rotatedY+position.latitude, rotatedZ + position.altitude });
        }
        
    }
    return points;
}

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ObstaclePublisher>());
    rclcpp::shutdown();
    return 0;
}

//int main() {
//    // 示例：创建一个 ObstructionStruct 实例
//    std::vector<double> ref_position = { 116.3971281, 39.9165271, 0.0 };
//    std::vector<double> obs_position;
//    ObstructionStruct obstruction;
//    SCGPositionGEO drone_p;
//    obstruction.shape = 3;
//    obs_position = wgs84_to_enu({ 116.3971281, 39.9165271, 0.0 }, ref_position[0], ref_position[1], ref_position[2]);
//    obstruction.position = { obs_position[0], obs_position[1] - 5, obs_position[2]+6.5 };
//    drone_p = { obs_position[0], obs_position[1] - 20, obs_position[2] +3.5 };
//    obstruction.velocity = { 5.0, 0.0, 0.0 }; // 示例速度
//    obstruction.attitude = { 30, 40, 50 }; // 示例姿态 (以弧度表示)
//    obstruction.paraA = 6.0; // 球形半径
//    obstruction.paraB = 3.0; // 不适用
//    obstruction.paraC = 3.0; // 不适用
//
//    // 计算表面点
//    auto surfacePoints = obstruction.computeSurfacePoints(1.0, drone_p, 100.0);
//
//    // 输出示例
//    for (const auto& point : surfacePoints) {
//        std::cout << point[0] << ", " << point[1] << ", " << point[2] << ";" << std::endl;
//    }
//
//    return 0;
//}
